# -*- coding: utf-8 -*-
import math
import random
import time
import numpy as np
import matplotlib.pyplot as plt
import EventLog as log

#肘部关节
class ElbowJoint:
    def __init__(self):
        self.zerol=209.5 #零点长度
        self.minl=209 #最小长度
        self.max=263.5  #最大长度
        self.side_h=38.18 #横向长度
        self.side_v=231.35 #竖向长度
        self.side_rod=209.5 #丝杆长度
        self.xlist = []
        self.ylist = []
        self.der = 51  #转动角度+初始角度 = 机器人关节角度
        for l in range(20900, 26350, 10):
            l1 = l * 1.0 / 100
            j4 = self.calculate_ElowChangeAngle(l1)
            self.xlist.append(l1)
            self.ylist.append(j4)

    def calculate_ElowChangeAngle(self,rod_length):
        angle_A,_,_ = self.calculate_Angles(self.side_rod, self.side_h, self.side_v)
        change_angel_A,_,_ = self.calculate_Angles(rod_length, self.side_h, self.side_v)

        angel4 = change_angel_A - angle_A

        return angel4

    def find_nearest_number(self,lst, target):
        nearest_number = min(lst, key=lambda x: abs(x - abs(target)))
        index = lst.index(nearest_number)
        return nearest_number, index

    #已知三边求三角度
    def calculate_Angles(self,side_a,side_b,side_c):
        #print("side_:",side_a,side_b,side_c)
        #余弦定理
        angle_A = math.acos((side_b**2+side_c**2-side_a**2)/(2*side_b*side_c))
        angle_B = math.acos((side_a ** 2 + side_c ** 2 - side_b ** 2) / (2 * side_a * side_c))
        angle_C = math.pi - angle_A - angle_B

        # 将弧度转换为角度
        angle_A = math.degrees(angle_A)
        angle_B = math.degrees(angle_B)
        angle_C = math.degrees(angle_C)

        return angle_A, angle_B, angle_C

    # 已知丝杆的变化长度求关节变化的角度
    def Length2JointAngle(self,change_rod):
        # 给出电机的长度，关节的角度
        nearest_number, index = self.find_nearest_number(self.xlist, change_rod)
        return self.ylist[index]

    def JointAngle2Length(self,change_angle):
        # 给出一个关节角度，得到点击长度
        nearest_number, index = self.find_nearest_number(self.ylist, change_angle)
        return self.xlist[index]

    # 已知丝杆的变化长度求关节变化的角度
    def Length2JointAngle2(self,change_rod):
        angle_4 = self.calculate_ElowChangeAngle(change_rod)
        return angle_4

    # 给出一个关节角度，得到点击长度
    def JointAngle2Length2(self,change_angle):

        angle_A, _, _ = self.calculate_Angles(self.side_rod, self.side_h, self.side_v)
        rod_angel_=change_angle + angle_A
        rod_angel_ = math.radians(rod_angel_)
        rod_length=self.side_h**2+self.side_v**2-2*self.side_h*self.side_v*math.cos(rod_angel_)
        rod_length=math.sqrt(rod_length)

        return rod_length

#手腕部关节
class WristJoint:
    def __init__(self):
        self.zerol = 174.42  # 零点长度
        self.minl = 148  # 最小长度
        self.max = 192  # 最大长度
        self.side_h=167.8 #横向长度
        self.side_v=22.56 #竖向长度
        self.side_rod=174.42 #丝杆长度
        self.xlist = []
        self.ylist = []

        self.der7 = 103  # 转动角度+初始角度 = 机器人关节角度

        for la in range(1480, 1900, 1):
            for lb in range(1480, 1900, 1):
                la1 = la * 1.0 / 10
                lb1 = lb * 1.0 / 10
                j6, j7 = self.calculate_AnkleChangeAngle(la1, lb1)
                self.xlist.append((la1, lb1))
                self.ylist.append((j6, j7))

    def calculate_AnkleChangeAngle(self,rod_length1,rod_length2):
        rod_length_ =0.5*( rod_length1 + rod_length2)
        #print("rod_length_",rod_length_)
        rod_length_m=rod_length1-rod_length2
        _,_,angle_A = self.calculate_Angles( self.side_h, self.side_v,rod_length_)

        angel7 = angle_A #俯仰角

        angel6=math.degrees(math.asin(0.5*rod_length_m/31)) #偏航角度

        #angel6 = math.degrees(0.5 * rod_length_m / 31)  # 偏航角度

        return angel6,angel7

    def find_nearest_number(self,points, px,py):
        p = [px, py]
        min_distance = math.inf
        nearest_index = None

        for i, point in enumerate(points):
            distance = math.sqrt((point[0] - p[0]) ** 2 + (point[1] - p[1]) ** 2)
            if distance < min_distance:
                min_distance = distance
                nearest_index = i

        return nearest_index

    # 已知三边求三角度
    def calculate_Angles(self, side_a, side_b, side_c):
        # 余弦定理
        #print(side_a,side_b,side_c)
        angle_A = math.acos((side_b ** 2 + side_c ** 2 - side_a ** 2) / (2 * side_b * side_c))
        angle_B = math.acos((side_a ** 2 + side_c ** 2 - side_b ** 2) / (2 * side_a * side_c))
        angle_C = math.pi - angle_A - angle_B


        # 将弧度转换为角度
        angle_A = math.degrees(angle_A)
        angle_B = math.degrees(angle_B)
        angle_C = math.degrees(angle_C)
        #print(angle_A)
        return angle_A, angle_B, angle_C

    # 给出关节角度，得到点击长度
    def JointAngle2Length(self, angle6, angle7):
        # if angle7 > 10:
        #     log.logger.warning("左右摆动角度不能大于18，实际为%s" % angle7)
        #     in_angle6 = 10
        # if angle6 < -10:
        #     log.logger.warning("左右摆动角度不能大于18，实际为%s" % angle6)
        #     angle6 < -10
        if angle6>10:angle6=10
        elif angle6<-10:angle6=-10

        index = self.find_nearest_number(self.ylist, angle6,angle7+self.der7)
        return self.xlist[index][0], self.xlist[index][1]

    # 计算给出关节角度，得到点击长度
    def JointAngle2Length2(self, angle6, angle7):
        radians6 = math.radians(angle6)
        radians7 = math.radians(self.der7-angle7)
        rod_length_=62*math.sin(radians6)

        length_m_=self.side_h**2+self.side_v**2-2*self.side_h*self.side_v*math.cos(radians7)
        length_m_=math.sqrt(length_m_)

        length_1=0.5*rod_length_+length_m_
        length_2=2*length_m_-length_1

        return length_1,length_2


    # 已知丝杆的变化长度求关节变化的角度
    def Length2JointAngle(self, change_rod1,change_rod2):
        index = self.find_nearest_number(self.xlist, change_rod1, change_rod2)
        return self.ylist[index][0], self.ylist[index][1]-self.der7

    # 计算已知丝杆的变化长度求关节变化的角度
    def Length2JointAngle2(self, change_rod1,change_rod2):
        angel6, angel7 = self.calculate_AnkleChangeAngle(change_rod1,change_rod2)
        return angel6, angel7

#肘关节测试
def test_ElbowJoint():
    elbow=ElbowJoint()
    #测试肘变化角度
    change_rod=240
    angle4=elbow.calculate_ElowChangeAngle(change_rod)
    print(angle4)
    minangle = elbow.calculate_ElowChangeAngle(209.5)
    maxangle = elbow.calculate_ElowChangeAngle(263.5)
    print("最小角度：", minangle, "丝杆最小长度：", 209.5)
    print("最大角度：", maxangle, "丝杆最大长度：", 263.5)


    # 2.2.通过长度计算关节变化角度（根据angleList和lenghtList取最邻近点）

    rlt_angle = elbow.Length2JointAngle2(263.5)
    print(rlt_angle)
    # 2.3.通过长度计算角度（根据angleList和lenghtList取最邻近点）
    #方法1
    # rlt_lenght = elbow.JointAngle2Length(20)
    # print("1",rlt_lenght)
    # angle_=elbow.Length2JointAngle(rlt_lenght)
    # print("1",angle_)
    # 方法2
    rlt_lenght2 = elbow.JointAngle2Length(20)
    print("2",rlt_lenght2)
    angle2_=elbow.Length2JointAngle(rlt_lenght2)
    print("2",angle2_)
    # length_=elbow.JointAngle2Length(50)
    # print(length_)

    lenghtList = elbow.xlist
    angleList = elbow.ylist
    y1list = []
    y2list = []
    for x,y in zip(lenghtList,angleList):
        y1list.append(x)
        y2list.append(y)
    plt.scatter(y1list, y2list)
    plt.xlabel('length')
    plt.ylabel('angle4')
    plt.show()

    del elbow
#测试腕关节
def test_WristJoint():
    wrist=WristJoint()
    angle6,angle7=wrist.calculate_AnkleChangeAngle(180,180)
    print(angle6,angle7)
    minangle6, minangle7 = wrist.calculate_AnkleChangeAngle(148, 148)
    print("最小最小角度：", minangle6, minangle7, "丝杆长度：", 148, 148)
    minangle6, minangle7 = wrist.calculate_AnkleChangeAngle(190, 148)
    print("最大最小角度：", minangle6, minangle7, "丝杆长度：", 190, 148)
    minangle6, minangle7 = wrist.calculate_AnkleChangeAngle(148, 190)
    print("最小最大角度：", minangle6, minangle7, "丝杆长度：", 148, 190)
    minangle6, minangle7 = wrist.calculate_AnkleChangeAngle(190, 190)
    print("最大最大角度：", minangle6, minangle7, "丝杆长度：", 190, 190)

    minangle6, minangle7 = wrist.calculate_AnkleChangeAngle(170+20, 170-20)
    print("踝关节摆动角度：", minangle6, minangle7, "丝杆长度：", 190, 150)

    length6_,length7_=wrist.JointAngle2Length(3.2,-13)

    print("1111",length6_,length7_)
    angle6_,angle7_=wrist.Length2JointAngle(length6_,length7_)
    print(angle6_,angle7_)

    length6_,length7_=wrist.JointAngle2Length2(3.2,-13)
    print("2222",length6_,length7_)

    angle6_,angle7_=wrist.Length2JointAngle2(length6_,length7_)
    print(angle6_,wrist.der7-angle7_)

    lenghtList = wrist.xlist
    angleList = wrist.ylist
    y1list = []
    y2list = []
    for y in angleList:
        if -30<=y[0]<=30:
            y1list.append(y[0])
            y2list.append(y[1]-103)
    # plt.scatter(y1list, y2list)
    # plt.xlabel('angle6')
    # plt.ylabel('angle7')

    fig1 = plt.figure()
    ax1 = fig1.add_subplot(111)
    ax1.scatter(y2list, y1list)
    plt.xlabel('angle6')
    plt.ylabel('angle7')


    plt.show()
    # # 绘制折线图
    # for i in range(len(lenghtList)):
    #     plt.plot([lenghtList[i][0], lenghtList[i][1]], [angleList[i][0], angleList[i][1]], marker='o')
    #
    # plt.xlabel('X-axis')
    # plt.ylabel('Y-axis')
    # plt.title('Two Sets of Two-Dimensional Coordinates')
    # plt.show()  # 显示图形






    del wrist

if __name__=='__main__':
    #test_ElbowJoint()
    test_WristJoint()